
#include "ConvexPolygon.h"
#include "Transform2.h"
#include "Entity.h"

namespace p2 {

IMPLEMENT_RTT(p2, ConvexPolygon);

ConvexPolygon::~ConvexPolygon()
{
}

ConvexPolygon* ConvexPolygon::create(real mass, Vector2<real> position, real rotation, p2Material const& material, uint32 plane, void* userData)
{
	ConvexPolygon* newPolygon = _allocator.allocate(1);
	if (newPolygon) {
		_allocator.construct(newPolygon, ConvexPolygon(mass, position, rotation, material, plane, userData));
	}
	return newPolygon;
}

bool ConvexPolygon::updateShapeData()
{
	/*
	 * validate is a polygon
	 */
	uint32 const sizeV = _localVertices.size();
	if (sizeV < 3) {
		return false;
	}
	/*
	 * validate is convex
	 */
	_centerOfMass = Vector2<real>::ZERO;
	for (uint32 vi = 0; vi < sizeV; vi++) {
		_centerOfMass += _localVertices[vi];
	}
	_centerOfMass /= (real) sizeV;
	for (uint32 vi = 0; vi < sizeV; vi++) {
		Vector2<real> a = _localVertices[vi] - _centerOfMass;
		Vector2<real> b = _localVertices[(vi + 1) % sizeV] - _centerOfMass;
		Vector2<real> c = _localVertices[(vi + 2) % sizeV] - _centerOfMass;
		if ((b - a).perp().dot(c - b) > 0.f) {
			return false;
		}
	}
	_localNormals.reserve(sizeV);
	real sumI1 = 0.f;
	real sumI2 = 0.f;
	for (uint32 vi = 0; vi < sizeV; vi++) {
		Vector2<real>& v1(_localVertices[vi]);
		Vector2<real>& v2(_localVertices[(vi + 1) % sizeV]);
		real a = v2.perp().dot(v1);
		real b = Vector2<real>::dot(v1, v1) + Vector2<real>::dot(v1, v2) + Vector2<real>::dot(v2, v2);
		sumI1 += a * b;
		sumI2 += a;
		Vector2<real> n = Vector2<real>().makeNormal(v1, v2);
		_localNormals.push_back(n);
	}
	if (getInvMass() != 0.f) {
		real I = (getMass() * sumI1) / (6.f * sumI2);
		setInertia(I);
	}
	_volume = sumI2 * 0.5f;

	return true;
}

void ConvexPolygon::updateMotionBounds(real64 dT)
{
	Matrix4<real> fmvm(Element::getFutureWorldMatrix(dT));
	uint32 sizeV = _localVertices.size();
	_motionBounds.reset();
	for (uint32 vi = 0; vi < sizeV; vi++) {
		Vector4<real> fwv(Vector4<real>(_localVertices[vi]) * fmvm);
		_motionBounds.growToContain(Vector2<real>(fwv.X(), fwv.Y()));
	}
	_motionBounds.growToContain(_worldBounds);
}

void ConvexPolygon::updateWorldSpace()
{
	updateTransform();

	uint32 sizeV = _localVertices.size();
	_worldVertices.resize(sizeV);
	_worldBounds.reset();
	_worldNormals.resize(sizeV);
	Matrix2<real> rot = _transform.getRotation() * _owner->getTransform().getRotation();
	for (uint32 vi = 0; vi < sizeV; vi++) {
		Vector4<real> wv4(Vector4<real>(_localVertices[vi]) * _worldMatrix);
		_worldVertices[vi] = Vector2<real>(wv4.X(), wv4.Y());
		_worldBounds.growToContain(_worldVertices[vi]);
		_worldNormals[vi] = _localNormals[vi] * rot;
	}
}

/*
 * REF: http://paulbourke.net/geometry/insidepoly/
 * assumes convex polygon with anti clockwise winding
 */
bool ConvexPolygon::contains(Vector2<real> worldPoint)
{
    uint32 vsize = _worldVertices.size();
    for ( uint32 i = 0; i < vsize; i++ )
    {
        Vector2<real>& p0 = _worldVertices[i];
        Vector2<real>& p1 = _worldVertices[(i+1)%vsize];
        /*
         * (y - y0) (x1 - x0) - (x - x0) (y1 - y0)
         * '.. is less than 0 then P is to the right of the line segment, if greater
         * than 0 it is to the left, if equal to 0 then it lies on the line segment'
         */
        real side = (worldPoint.Y() - p0.Y()) * (p1.X() - p0.X()) - (worldPoint.X() - p0.X()) * (p1.Y() - p0.Y());
        if ( side < 0.f ) {
            return false;
        }
    }
    return true;
}

int32 ConvexPolygon::getSupportingVertices(Vector2<real> const& nv, uint32 (&svi)[2])
{
	Vector2<real> lsn = getOwner()->getTransform().rotateIntoLocalSpace(nv);
	real first = -Math<real>::MAX_REAL;
	real second = -Math<real>::MAX_REAL;
	int32 firstI = -1, secondI = -1;
	uint32 sizeV = _localVertices.size();
	for (uint32 i = 0; i < sizeV; i++) {
		Vector2<real>& rx = _localVertices[i];
		register real d = lsn.dot(rx);
		if (d >= first) {
			second = first;
			secondI = firstI;
			first = d;
			firstI = i;
		} else if (d > second) {
			second = d;
			secondI = i;
		}
	}
	svi[0] = firstI;
	if ((first - second) == 0.f) {
		svi[1] = secondI;
		return 2;
	}
	return 1;
}

void ConvexPolygon::makeTriangle(real halfWidth, real halfHeight)
{
	addVertex(-halfWidth, -halfHeight);
	addVertex(+halfWidth, -halfHeight);
	addVertex(0.f, +halfHeight);
    _shapeHint = TrianglePolygonShape;
    setHalfExtent(Vector2<real>(halfWidth, halfHeight));
}

void ConvexPolygon::makeBox(real halfWidth, real halfHeight)
{
	addVertex(-halfWidth, -halfHeight);
	addVertex(+halfWidth, -halfHeight);
	addVertex(+halfWidth, +halfHeight);
	addVertex(-halfWidth, +halfHeight);
    _shapeHint = BoxPolygonShape;
    setHalfExtent(Vector2<real>(halfWidth, halfHeight));
}

void ConvexPolygon::makeRegular(real halfWidth, real halfHeight, uint32 numSides)
{
	ASSERTLOG(numSides>=5, "Polygon::makeRegular");
	real s = Math<real>::TWO_PI / (real) numSides;
	for (uint32 i = 0; i < numSides; i++) {
		real x = halfWidth * Math<real>::Cos((real) i * s);
		real y = halfHeight * Math<real>::Sin((real) i * s);
		addVertex(x, y);
	}
    _shapeHint = RegularPolygonShape;
    setHalfExtent(Vector2<real>(halfWidth, halfHeight));
}

}
